#include"vld_imu_slam/feature_extract.h"
#include"vld_imu_slam/show_pub.h"
#include"vld_imu_slam/frontend.h"

vld_imu_slam::FeatureExtract::Ptr p_feature_extract_;
vld_imu_slam::Frontend::Ptr p_frontend_;
vld_imu_slam::ShowPub::Ptr p_show_pub_;

//debug
int frame_counter_ = 0;

void pc_custom_CB(const sensor_msgs::PointCloud2ConstPtr &msg){
    // frame_counter_++;
    // if(frame_counter_>1&&frame_counter_<3)
    //     return;

    // if(frame_counter_>3)
    //     return;

    p_feature_extract_->pcHandle(msg);
    // vld_imu_slam::Frame::Ptr p_frame(new vld_imu_slam::Frame(p_feature_extract_->pc_full_,
    //                                                                 p_feature_extract_->pc_ground_,p_feature_extract_->pc_groundPt_sample_,
    //                                                                 p_feature_extract_->pc_conner_,p_feature_extract_->pc_cornerPt_sample_));
    vld_imu_slam::Frame::Ptr p_frame(new vld_imu_slam::Frame(p_feature_extract_->pc_full_,
                                                                p_feature_extract_->pc_surface_,p_feature_extract_->pc_conner_));
    p_frontend_->addFrame(p_frame);
   
}

void frontendThread(){
    while(true){
        if(p_frontend_->frontendHandle()){
            p_show_pub_->pubMap(p_frontend_->p_map_->getMap());
            // p_show_pub_->pubMap(p_frontend_->p_last_frame_->pc_surface_);
            // p_show_pub_->pubMap(p_feature_extract_->pc_surface_);
            // p_show_pub_->pubFull(p_feature_extract_->pc_full_);
            double var[7];
            vld_imu_slam::SE3toRt(p_frontend_->p_map_->p_current_frame_->getPose(),var);
            p_show_pub_->pubOdom(var);
        }
        ros::Duration(0.00001).sleep();
    }
}

int main(int argc, char** argv)
{
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "vld_imu_slam");
    ros::NodeHandle nh;

    vld_imu_slam::Config::Ptr p_config(new vld_imu_slam::Config(nh));
    p_feature_extract_.reset(new vld_imu_slam::FeatureExtract(p_config));

    p_frontend_.reset(new vld_imu_slam::Frontend(p_config));

    p_show_pub_.reset(new vld_imu_slam::ShowPub(nh));

    ros::Subscriber pc_sub_ = nh.subscribe<sensor_msgs::PointCloud2>(p_config->pointcloud_topic_, 5, pc_custom_CB, ros::TransportHints().tcpNoDelay());
    
    thread frontend_thread(frontendThread);
    frontend_thread.detach();
    // ros::MultiThreadedSpinner spinner(3);
    // spinner.spin();
    ros::spin();



    
    return 0;
}
